#include "MPC_Base_T.h"
#include "../../../ThirdPartyCode/NR_Vector_CVersion.h"
#include "../../../ThirdPartyCode/Utilities.h"
// =============================================
//
// Class: MPC_settings
//
// =============================================
MPC_settings::MPC_settings(int iHorizon)
{
	int i;
	Horizon = iHorizon;
	Weighting = new double[Horizon];
	for(i=0;i<Horizon;i++)
	{
		Weighting[i] = 1.0;
	}
	
	// Default parameters;
	Dens_set   = 0.98;
	Fact_D     = 1.0;
	Rough2_set = 60;
	Fact_R2    = 1.0;
	Ht_set     = 400;
	Fact_H     = 1.0;
	VarR2_set  = 0;
	Fact_VarR2 = 1.0; 
	ControllerID = 1;	
}
// =============================================
MPC_settings::~MPC_settings()
{
	delete [] Weighting;
}
// =============================================
/*int MPC_settings::set(char* VarName,double VarValue)
{
	if strcmp(VarName,"REPEAT") == 0
		Repeat = VarValue;
	else if strcmp(VarName,"Dens_set")==0
		Dens_set = VarValue;
	else if(strcmp(VarName,"Fact_D")==0)
		Fact_D = VarValue;
	else if(strcmp(VarName,"Rough2_set")==0)
		Rough2_set = VarValue;
	else if(strcmp(VarName,"Fact_R2")==0)
		Fact_R2 = VarValue;
	else if(strcmp(VarName,"varR2_set")==0)
		varR2_set = VarValue;
	else if(strcmp(VarName,"Fact_varR2")==0)
		Fact_varR2 = VarValue;
	else if(strcmp(VarName,"Ht_set")==0)
		Ht_set = VarValue;
	else if(strcmp(VarName,"Fact_h")==0)
		Fact_h = VarValue;
	else if(strcmp(VarName,"RT_dep")==0)
		RT_dep = VarValue;
	else if(strcmp(VarName,"DepRateDefault")==0)
		DepRateDefault = VarValue;

	return 1;
}*/
// =============================================


// =============================================
//
// Class: MPC_Base
//
// =============================================
MPC_Base::MPC_Base(	double	iDens_set,
					double	iFact_D, 
					double	iRough2_set,
					double	iFact_R2, 
					double	iHt_set,
					double	iFact_H, 
					double	it_end,
					double  iT_ini,
					double	iRT_T,
					double	it_int_ctrl, 
					int		iStep_num,
					int		iRel_err,
					char*   iFileName,
					int		iDetail,
					int		iControllerTypeID)
{
	int i;
	char columnname[1024];
	columnname[0] = '\0';


	if(iDens_set>1.0 || iDens_set < 0.0)		{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of density\n";system("pause");exit(0);}
	if(iFact_D < 0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of density\n";system("pause");exit(0);}
	if(iRough2_set < 0.0)						{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of roughness\n";system("pause");exit(0);}
	if(iFact_R2<0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of roughness\n";system("pause");exit(0);}
	if(iHt_set < 0.0)							{cerr << "Error@MPC_Base::MPC_Base(): illegal value of set point of height\n";system("pause");exit(0);}
	if(iFact_H < 0.0)			{cerr << "Error@MPC_Base::MPC_Base(): illegal value of weight of height. iFact_H = " << iFact_H << "\n";system("pause");exit(0);}
	if(iT_ini>1500 || iT_ini <0.0)				{cerr << "Error@MPC_Base::MPC_Base(): illegal value of DepRate. DepRate = " << iT_ini << "\n";system("pause");exit(0);}
	if(it_int_ctrl <= 0.0)						{cerr << "Error@MPC_Base::MPC_Base(): illegal value of t_int_ctrl. t_int_ctrl = " << it_int_ctrl << "\n";system("pause");exit(0);}
	if(iStep_num <= 0)							{cerr << "Error@MPC_Base::MPC_Base(): illegal value of Step_num. Step_num = " << iStep_num << "\n";system("pause");exit(0);}
	if(iRel_err!=0 && iRel_err!=1)				{cerr << "Error@MPC_Base::MPC_Base(): Rel_err must be 1 or 0. Rel_err = "<< Rel_err << "\n";system("pause");exit(0);}

	t_int_ctrl	= it_int_ctrl;	
	t_begin		= 0.0;
	t_end		= it_end;
	Step_num	= iStep_num;			
	RT_T		= iRT_T;				
	Dens_set	= iDens_set;		
	Fact_D		= iFact_D;
	Rough2_set	= iRough2_set;
	Fact_R2		= iFact_R2;
	Ht_set		= iHt_set;			
	Fact_H		= iFact_H;
	Rel_err		= iRel_err;
	T_ini		= iT_ini;
	FileName    = iFileName;
	Detail		= iDetail;
	ControllerTypeID = iControllerTypeID;

	Repeated	= 0;
	t_idx		= 0;
	t_node		= int((t_end-t_begin)/t_int_ctrl)+1;

	// Recording variables
	t_arr	= new double[t_node];
	W_sum	= new double[t_node]; 
	W2_sum	= new double[t_node];
	T_sum   = new double[t_node];
	T2_sum  = new double[t_node];
//	T_detail = new signal("T",t_end,t_int_ctrl,1);
//	T_avg   = new averagesignal("T",t_end,t_int_ctrl,1);

	v_dep_rate  = vector(1,Step_num);
	v_T         = vector(1,Step_num);
	uplim		= vector(1,Step_num);
	lolim		= vector(1,Step_num);

	for(i = 0;i<t_node;i++)
	{
		t_arr[i]	= i*t_int_ctrl;
		W_sum[i]	= 0.0;
		W2_sum[i]	= 0.0;
		T_sum[i]    = 0.0;
		T2_sum[i]   = 0.0;
	}
	
	ht			= 0.0;
	R2			= 0.0;
	g_dens		= 0.0;
	//dep_rate	= DepRate_ini;	
	dep_rate    = 0.2;
	T           = T_ini;
	time		= 0.0;
	
	Model = new model(MODE,t_int_ctrl);

	MPC_diag.cost		= 0;
	MPC_diag.Dopt		= new double[Step_num];
	MPC_diag.Hopt		= new double[Step_num];
	MPC_diag.R2opt		= new double[Step_num];
	MPC_diag.M2opt      = new double[Step_num];
	MPC_diag.varR2opt	= new double[Step_num];
	MPC_diag.exitflag   = 0;
	MPC_diag.ub         = new double[Step_num];
	MPC_diag.lb         = new double[Step_num];

	if(Detail==1) 
	{
		fd_time.open("MPC_time_d.dat");
		fd_T.open("MPC_T_d.dat");
		fd_W.open("MPC_W_d.dat");
		fd_cost.open("MPC_cost_d.dat");
		fd_Hopt.open("MPC_Hopt_d.dat");
		fd_Dopt.open("MPC_Dopt_d.dat");
		fd_R2opt.open("MPC_R2opt_d.dat");
		fd_M2opt.open("MPC_M2opt_d.dat");
	}
}
// =============================================
MPC_Base::~MPC_Base()
{
	int i;
	double T_mean;
	double T_var;
	double T_var2;

	ofstream fout;
	fout.open(FileName);
	fout << "class: MPC_Base" << "\n";
	fout << "Dens_set    = " << Dens_set << "\n";
	fout << "Fact_D      = " << Fact_D << "\n";
	fout << "Rough2_set  = " << Rough2_set << "\n";
	fout << "Fact_R2     = " << Fact_R2 << "\n";
	fout << "Ht_set      = " << Ht_set << "\n";
	fout << "Fact_Ht     = " << Fact_H << "\n";
	fout << "t_int_ctrl  = " << t_int_ctrl << "\n";
	fout << "Step_num    = " << Step_num << "\n";
	fout << "RT_T        = " << RT_T << "\n";
	fout << "T_ini       = " << T_ini << "\n";
	fout << "Rel_err     = " << Rel_err << "\n";
	fout << "Repeated    = " << Repeated << "\n";
	fout << setw(6) << "Time(s)" << setw(15) << "<T>(K)" << setw(18) << "Var(T)(K^2)" << "\n";
	for(i = 0;i<t_node;i++)
	{
		T_mean = T_sum[i]/Repeated;
		
		T_var2 = (T2_sum[i]-T_mean*T_mean*Repeated)/Repeated;
		if(T_var2 < -TOLERANCE)
		{
			cerr << "ERROR@MPC_Base::~MPC_Base().\n";
			cerr << " i=" << i << "Sum(T2)=" << T2_sum[i] << "<T>=" << T_mean << "Var(T)^2=" << T_var2 << "\n";
		}
		else if (T_var2 > -TOLERANCE && T_var2 < 0)
		{
			T_var = 0.0;
		}
		else
			T_var = sqrt(T_var2);
		
		fout << setw(6)  << setprecision(10) << t_arr[i];
		fout << setw(15) << setprecision(10) << T_mean;
		fout << setw(18) << setprecision(10) << T_var;
		if(i<t_node-1)  fout << "\n";
	}
	fout.close();

	if(Detail==1) 
	{
		fd_T.close();
		fd_W.close();
		fd_cost.close();
		fd_Hopt.close();
		fd_Dopt.close();
		fd_R2opt.close();
		fd_M2opt.close();
	}

	delete [] W_sum;
	delete [] W2_sum;
	delete [] T_sum;
	delete [] T2_sum;
	delete [] MPC_diag.Hopt;
	delete [] MPC_diag.Dopt;
	delete [] MPC_diag.R2opt;
	delete [] MPC_diag.M2opt;
	delete [] MPC_diag.varR2opt;
	delete [] MPC_diag.ub;
	delete [] MPC_diag.lb;

	free_vector(lolim,1,Step_num);
	free_vector(uplim,1,Step_num);
	free_vector(v_dep_rate,1,Step_num);
	free_vector(v_T,1,Step_num);

	delete Model;
}
// =============================================
void MPC_Base::reset()
{
	ht		= 0.0;
	//rough	= 0.0;
	R2		= 0.0;
	g_dens	= 0.0;	
	dep_rate	= 0.2;
	T           = T_ini;
	time		= 0.0;
	t_idx		= 0;
	
	W_sum[t_idx] += dep_rate;
	W2_sum[t_idx] +=  dep_rate*dep_rate;
	T_sum[t_idx] += T;
	T2_sum[t_idx] += T*T;

	t_idx ++;
	Repeated += 1;
}
// =============================================
int MPC_Base::notfinish()
{
	return t_idx < t_node;
}
// =============================================
void MPC_Base::log_detail()
{
	int i;
	
	for(i=1;i<=Step_num;i++)
	{
		fd_T << setw(20) << setprecision(10) << v_T[i];
	}
	fd_T << "\n";

	fd_cost << MPC_diag.cost << "\n";

	for(i=0;i<Step_num;i++)
	{
		fd_Hopt << setw(16) << MPC_diag.Hopt[i];
	}
	fd_Hopt << "\n";

	for(i=0;i<Step_num;i++)
	{
		fd_Dopt << setw(16) << MPC_diag.Dopt[i];
	}
	fd_Dopt << "\n";

	for(i=0;i<Step_num;i++)
	{
		fd_R2opt << setw(16) << MPC_diag.R2opt[i];
	}
	fd_R2opt << "\n";

	fd_M2opt << setw(16) << MPC_diag.M2_0;
	for(i=0;i<Step_num;i++)
	{
		fd_M2opt << setw(16) << MPC_diag.M2opt[i];
	}
	fd_M2opt << "\n";
}
// =============================================



// =============================================
//
// UPPER AND LOWER BOUND
//
// =============================================
double MPC_Base::cost_funct(double DEP_RATE[], double para[]) 
// cost_funct will calculate the deviation of SOR and roughness from the set-point values
// temp is the temperature of the substrate
{
	int		i,j;
	double	n;
	int		step_num;
	double	lambda,phi2_ss;	// tau is time constant, phi2_ss is the steady-state value of alpha^2
	double	dens_curr,rough2_curr,ht_curr,varR2_pred;		// values before predictions
	double	dens_pred,rough2_pred,ht_pred;		// sum of predictions at prediction horizons
	double	var0=0.0,cur_time;
	double	dev_total=0,dev_r2,dev_d,dev_h,dev_varR2;
	double	alpha2_pred[MODE],beta2_pred[MODE];		// for cost_funct use only
	double	alpha2_curr[MODE],beta2_curr[MODE];		// for cost_funct use only

	step_num=(int)para[1];						

	// assign current states to the predicting variables
	for(j=0;j<MODE;j++)
	{
		alpha2_curr[j]	= alphas[j]*alphas[j];
		beta2_curr[j]	= betas[j]*betas[j];
	}
	ht_curr=ht;
	dens_curr=g_dens;

	// dev_total = \sum_{i=1}{p}{Fact_D*\frac{
	dev_total=0.0;
	for(i=1;i<=step_num;i++)
	{
		Model->update(DEP_RATE[i]);
		
		// Thickness ---------------------------------
		ht_pred  = Model->getH();
		// if the predicted thickness is less than the set-point, add the deviation square to the cost
		// otherwise do nothing, i.e., penalizing on the under-growth film
		if(ht_pred<Ht_set)	
		{
			dev_h=ht_pred-Ht_set;
		}
		else
		{
			dev_h=0;
		}
		if (Rel_err)
		{
			dev_h/=Ht_set;	// relative error
		}
		dev_h*=dev_h;
		// Porosity ---------------------------------
		dens_pred   = Model->getRho();
		dev_d=dens_pred-Dens_set;
		if (Rel_err)
		{
			dev_d/=Dens_set;	// relative error
		}
		dev_d*=dev_d;		
		// mean value of roughness square -----------
		rough2_pred  = Model->getR2();
		dev_r2=rough2_pred-Rough2_set;
		if (Rel_err)
		{
			dev_r2/=Rough2_set;	// relative error
		}
		dev_r2*=dev_r2;
		// variance of R2 ---------------------------
		varR2_pred = Model->getVarR2();
		dev_varR2  = varR2_pred*varR2_pred;

		dev_total+=Fact_H*dev_h+Fact_D*dev_d+Fact_R2*dev_r2+Fact_varR2*dev_varR2;
	}
	return dev_total;
}
// =============================================
void MPC_Base::upperlimit()
// upper bound
{
	int i;
	double T_upper;

	for(i=1;i<=Step_num;i++)
	{
		T_upper=T+i*t_int_ctrl*RT_T;
		uplim[i]=T_HIGH>T?T_upper:T_HIGH;
	}
}
// =============================================
void MPC_Base::lowerlimit()
// lower bound without height constraints
{
	int i;
	double T_lower;
	for(i=1;i<=Step_num;i++)
	{
		T_lower=T-i*t_int_ctrl*RT_T;
		lolim[i] = T_LOW>T_lower?T_LOW:T_lower;
	}
}
// =============================================

